#include <string.h>
#include "interpolate.h"

/*
 *  This algorithm is equivalent to
 *  - zero stuffing (inserting a zero between two samples)
 *  - lowpassfiltering with a half-band FIR filter
 *
 *  Zero stuffing and lowpass filtering is conceptually the
 *  "proper way" to do resampling.
 *
 *  The "half-band" property comes in handy because it allows us
 *  to get away with only half the number of multiplications
 *  during the application of the filter since nearly every second
 *  sample of its impulse response is zero.
 *
 *  The impulse response of the lowpass filter scaled by 1024 is:
 *  -7 0 42 0 -149 0 626 1024 626 0 -149 0 42 0 -7
 *
 */

static void twice_internal(
	int num_samples_in,
	const int16_t in[],
	int16_t out[])
{
	int_least32_t tmp;
	while (num_samples_in>0) {
		/* let's compute the "sample in-between" ... */
		tmp = (512
			-  7*((int_least32_t)in[0]+in[7])
			+ 42*((int_least32_t)in[1]+in[6])
			-149*((int_least32_t)in[2]+in[5])
			+626*((int_least32_t)in[3]+in[4])) >> 10;
		/* and clip it ... */
		*out++ = tmp<-32768 ? -32768 : (tmp>32767 ? 32767 : tmp);
		/* direct output of original sample ... */
		*out++ = in[4];

		++in;
		--num_samples_in;
	}
}

extern void twice_process(
	struct twice_state *s,
	int num_samples_in,
	const int16_t in[],
	int16_t out[])
{
	/* into temp we move the last samples of the previous block
	 * and copy the first samples of the new block to have them
	 * in a continuous buffer. */
	int16_t temp[14];
	int i; /* number of samples to copy, 7 at max */
	i = num_samples_in<7 ? num_samples_in : 7;
	memcpy(temp  ,s->memory,sizeof(s->memory));
	memcpy(temp+7,in,i*sizeof(*temp));

	/* twice internal needs a continuous input sample buffer */
	twice_internal(i,temp,out);
	if (num_samples_in<=7) {
		/* we're done. let's remember the last samples for next time */
		memcpy(s->memory,temp+i,7*sizeof(*temp));
	} else {
		/* for the remaining samples we can now directly
		 * work on the input array */
		twice_internal(num_samples_in-i,in,out+i*2);
		/* we're done. let's remember the last samples for next time */
		memcpy(s->memory,in+(num_samples_in-7),sizeof(s->memory));
	}
}
